#include "DataStorageStepper.h"
#include <EEPROM.h>
#include "linkage_steps_factory.h"
#include "log.h"

DataStorageStepper::DataStorageStepper(Mail<>& mb):_mb(mb)
{

}
void DataStorageStepper::begin()
{
    
}
bool DataStorageStepper::load()
{
    if(this->read_data_storage_eof() != DATA_STORAGE_EOF){
        return false;
    }
    //载入模组头部信息
    for(uint32_t i=0;i<data_storage_header_size();i++){
         module_header_reg().data[i] = EEPROM.read(i);
    }
    
    //载入模组通用信息
    module_general_reg_t& general_reg = module_reg().module_general_reg;
    for(uint32_t i=(DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS);i<(DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS+DATA_STORAGE_MODULE_GENERAL_SIZE);i++){
       general_reg.data[i-DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS] = EEPROM.read(i);
    }
    //设置停止状态
    general_reg.current_state_reg.value = 0;
    //读取联动配置

    uint8_t m1_v = general_reg.module_general_info[0].total_linkage_steps_stats_reg.value;
    uint8_t m2_v = general_reg.module_general_info[1].total_linkage_steps_stats_reg.value;
    uint8_t m3_v = general_reg.module_general_info[2].total_linkage_steps_stats_reg.value;

    //配置M1
    uint32_t offset =1000;
    for(uint8_t i=0;i<m1_v;i++){
        auto x = linkage_steps_factory::setup_allocator().alloc();
        if(x==nullptr){
            log::debug("1.alloc error:%d\r\n",m1_v);
            return true;
        }
        x->memory.data[0] = EEPROM.read(offset+i*4);
        x->memory.data[1] = EEPROM.read(offset+i*4+1);
        x->memory.data[2] = EEPROM.read(offset+i*4+2);
        x->memory.data[3] = EEPROM.read(offset+i*4+3);
        linkage_steps_factory::linkage_steps_setups(0).push_back(x);
    }

    offset =1200;
    for(uint8_t i=0;i<m2_v;i++){
        auto x = linkage_steps_factory::setup_allocator().alloc();
        if(x==nullptr){
            log::debug("2.alloc error:%d\r\n",m2_v);
            return true;
        }
        x->memory.data[0] = EEPROM.read(offset+i*4);
        x->memory.data[1] = EEPROM.read(offset+i*4+1);
        x->memory.data[2] = EEPROM.read(offset+i*4+2);
        x->memory.data[3] = EEPROM.read(offset+i*4+3);
        linkage_steps_factory::linkage_steps_setups(1).push_back(x);
    }

    offset =1400;
    for(uint8_t i=0;i<m3_v;i++){
        auto x = linkage_steps_factory::setup_allocator().alloc();
        if(x==nullptr){
            log::debug("3.alloc error::%d\r\n",m3_v);
            return true;
        }
        x->memory.data[0] = EEPROM.read(offset+i*4);
        x->memory.data[1] = EEPROM.read(offset+i*4+1);
        x->memory.data[2] = EEPROM.read(offset+i*4+2);
        x->memory.data[3] = EEPROM.read(offset+i*4+3);
        linkage_steps_factory::linkage_steps_setups(2).push_back(x);
    }

    return true;

}
int DataStorageStepper::read_data_storage_eof()
{
    int eof_address= DATA_STORAGE_EOF_ADDRESS;
    _data_storage_eof._AA = EEPROM.read(eof_address);
    _data_storage_eof._55 = EEPROM.read(eof_address+1);
    return _data_storage_eof.value;
}

void DataStorageStepper::make_default()
{
     //生成头部信息
    module_header_reg().module_number.value      = 0x0007;
    module_header_reg().hardware_version         = 0x01;
    module_header_reg().software_version         = 0x01;
    module_header_reg().module_type              = 1;
    module_header_reg().protocol_part_1.value    = 0x87;
    module_header_reg().protocol_part_2.value    = 0x01;
    strcpy((char*)& module_header_reg().module_name[0],"WF1-Stepper_Motor");
    
    //写入头部信息到EEPROM
    for(uint8_t i=0;i<data_storage_header_size();i++){
        EEPROM.write(i, module_header_reg().data[i]); 
    }

    module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
    //生成寄存器默认配置
    general_reg.master_update_data_reg.value       = 0;
    general_reg.slave_interrupt_reg.value          = 0;
    general_reg.control_enable_reg.value           = 0;
    general_reg.slave_update_setup_reg.value       = 0;
    general_reg.control_enable_reg.value           = 0;
    general_reg.motion_mode_reg.value              = 0;
    general_reg.current_state_reg.value            = 0;
    general_reg.motion_trigger_reg.value           = 0;
    //默认写入M1/M2/M3相同配置信息
    for(uint8_t i=0;i<DATA_STORAGE_MOTOR_NUM;i++){
        //general_reg.module_general_info[i].current_motion_steps_stats_reg
        general_reg.module_general_info[i].total_motion_steps_stats_reg.value            = 0x00;
        general_reg.module_general_info[i].acc_and_dec_setup_reg.value                   = 0x10;
        general_reg.module_general_info[i].total_linkage_steps_stats_reg.value           = 0x03;
        general_reg.module_general_info[i].total_cycle_count_setup_reg.value           = 0x00;
        general_reg.module_general_info[i].step_resolution_setup_reg.value               = 0x00;
        general_reg.module_general_info[i].current_limit_setup_reg.value                 = 0x00;
    }
    //写入默认配置到EEPROM
    for(uint32_t i=(DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS);i<(DATA_STORAGE_MODULE_GENERAL_SIZE+DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS);i++){
        EEPROM.write(i, general_reg.data[i-DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS]);
    }
    //写入0x55AA激活配置
    EEPROM.write(DATA_STORAGE_EOF_ADDRESS,0xAA);
    EEPROM.write(DATA_STORAGE_EOF_ADDRESS+1,0x55);
}

void DataStorageStepper::check_update()
{
    bool _update = false;

    for(uint32_t i=0;i<data_storage_header_size();i++){
        if(!_mb.empty()){
            return;
        }
        if(EEPROM.read(i) !=  module_header_reg().data[i]){
            _update = true;
            break;
        }
    }

    if(_update){
        for(uint32_t i=0;i<data_storage_header_size();i++){
            if(!_mb.empty()){
                return;
            }
            EEPROM.write(i, module_header_reg().data[i]);
        }
        //log::debug("update eeprom header:%s\r\n",_update?"Y":"N");
    }

    _update = false;
    
    for(uint32_t _check_index=DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS;_check_index<(DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS+DATA_STORAGE_MODULE_GENERAL_SIZE);_check_index++){
        if(!_mb.empty()){
            return;
        }
        module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
        if(EEPROM.read(_check_index) !=  general_reg.data[_check_index-DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS]){
            _update = true;
            break;
        }
    }

    if(_update){
        //写入默认配置到EEPROM
        module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
        for(uint32_t i=(DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS);i<(DATA_STORAGE_MODULE_GENERAL_SIZE+DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS);i++){
            if(!_mb.empty()){
                return;
            }
            EEPROM.write(i,general_reg.data[i-DATA_STORAGE_MODULE_GENERAL_BEGIN_ADDRESS]);
        }
        log::debug("update eeprom reg:%s\r\n",_update?"Y":"N");
    }

}


data_storage_reg_read_t& DataStorageStepper::get_reg_data(uint8_t reg_address)
{
    module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
    switch (reg_address)
    {
        case 0x00:
        case 0x01:
        {
            _reg_read.data    =  module_header_reg().module_number.data;
            _reg_read.size    = sizeof( module_header_reg().module_number.data);
        }
        break;
        case 0x02:
        {
            _reg_read.data    =  module_header_reg().hardware_version_data;
            _reg_read.size    = sizeof( module_header_reg().hardware_version_data);
        }
        break;
        case 0x03:
        {
            _reg_read.data    =  module_header_reg().software_version_data;
            _reg_read.size    = sizeof( module_header_reg().software_version_data);
        }
        break;
        case 0x04:
        {
            _reg_read.data    =  module_header_reg().module_type_data;
            _reg_read.size    = sizeof( module_header_reg().module_type_data);
        }
        break;
        case 0x05:
        case 0x06:
        {
            _reg_read.data    =  module_header_reg().protocol_data;
            _reg_read.size    = sizeof( module_header_reg().protocol_data);
        }
        break;
        case 0x07:
        {
            _reg_read.data    =  module_header_reg().module_name;
            //_reg_read.size    = sizeof( module_header_reg().module_name);
             _reg_read.size    = 7;
        }
        break;
    case 0x40:
        {
            //保留：数据可用寄存器
        }
        break;
    case 0x41://中断寄存器
        {
            _reg_read.data    =  general_reg.slave_interrupt_reg.data;
            _reg_read.size    =  sizeof(general_reg.slave_interrupt_reg.data);
            //清楚中断
        }
        break;
    case 0x42://设置更新寄存器:只写
        // {
        //     _reg_read.data    = (uint8_t*)"NG";
        //     _reg_read.size    = 2;
        // }
        break;
    case 0x43://使能驱动器
        {
            _reg_read.data    =  general_reg.control_enable_reg.data;
            _reg_read.size    = sizeof(general_reg.control_enable_reg.data);
        }
        break;
    case 0x44://运动模式
        {
            _reg_read.data    =  general_reg.motion_mode_reg.data;
            _reg_read.size    = sizeof( general_reg.motion_mode_reg.data);
        }
        break;
    case 0x45://保留
        {
           
        }
        break;
    case 0x46://M1总的累计运动步数
    case 0x47://M1总的累计运动步数
    case 0x48://M1总的累计运动步数
    case 0x49://M1总的累计运动步数
    {
        _reg_read.data    =  general_reg.module_general_info[0].total_motion_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].total_motion_steps_stats_reg.data);
    }
    break;
    case 0x4A://M2总的累计运动步数
    case 0x4B://M2总的累计运动步数
    case 0x4C://M2总的累计运动步数
    case 0x4D://M2总的累计运动步数
    {
         _reg_read.data    =  general_reg.module_general_info[1].total_motion_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].total_motion_steps_stats_reg.data);
    }
    break;
    case 0x4E://M3总的累计运动步数
    case 0x4F://M3总的累计运动步数
    case 0x50://M3总的累计运动步数
    case 0x51://M3总的累计运动步数
    {
        _reg_read.data    =  general_reg.module_general_info[2].total_motion_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].total_motion_steps_stats_reg.data);
    }
    break;
    case 0x52://M1加减速度
    case 0x53://M1加减速度
    {
        _reg_read.data    =  general_reg.module_general_info[0].acc_and_dec_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].acc_and_dec_setup_reg.data);
    }
    break;
    case 0x54://M2加减速度
    case 0x55://M2加减速度
    {
        _reg_read.data    =  general_reg.module_general_info[1].acc_and_dec_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].acc_and_dec_setup_reg.data);
    }
    break;
    case 0x56://M3加减速度
    case 0x57://M3加减速度
    {
        _reg_read.data    =  general_reg.module_general_info[2].acc_and_dec_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].acc_and_dec_setup_reg.data);
    }
    break;
    case 0x58://M1/M2/M3当前状态
    {
        _reg_read.data    =  general_reg.current_state_reg.data;
        _reg_read.size    = sizeof(general_reg.current_state_reg.data);
    }
    break;
    case 0x59://:M1联动步数
    {
        _reg_read.data    =  general_reg.module_general_info[0].total_linkage_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].total_linkage_steps_stats_reg.data);
    }
    break;
    case 0x5A://:M2联动步数
    {
        _reg_read.data    =  general_reg.module_general_info[1].total_linkage_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].total_linkage_steps_stats_reg.data);
    }
    break;
     case 0x5B://:M3联动步数
    {
        _reg_read.data    =  general_reg.module_general_info[2].total_linkage_steps_stats_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].total_linkage_steps_stats_reg.data);
    }
    break;
    case 0x5C://:M1联动循环次数
    case 0x5D://:M1联动循环次数
    {
        _reg_read.data    =  general_reg.module_general_info[0].total_cycle_count_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].total_cycle_count_setup_reg.data);
    }
    break;
    case 0x5E://:M2联动循环次数
    case 0x5F://:M2联动循环次数
    {
        _reg_read.data    =  general_reg.module_general_info[1].total_cycle_count_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].total_cycle_count_setup_reg.data);
    }
    break;
    case 0x60://:M3联动循环次数
    case 0x61://:M3联动循环次数
    {
        _reg_read.data    =  general_reg.module_general_info[2].total_cycle_count_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].total_cycle_count_setup_reg.data);
    }
    break;
    case 0x62://M(1-3)运动触发
    {
        _reg_read.data    =  general_reg.motion_trigger_reg.data;
        _reg_read.size    = sizeof(general_reg.motion_trigger_reg.data);
    }
    break;
    case 0x63://M1步进电机细分
    {
        _reg_read.data    =  general_reg.module_general_info[0].step_resolution_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].step_resolution_setup_reg.data);
    }
    break;
    case 0x64://M1步进电机细分
    {
        _reg_read.data    = general_reg.module_general_info[1].step_resolution_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].step_resolution_setup_reg.data);
    }
    break;
    case 0x65://M1步进电机细分
    {
        _reg_read.data    =  general_reg.module_general_info[2].step_resolution_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].step_resolution_setup_reg.data);
    }
    break;
    case 0x66://M1电机限流
    {
        _reg_read.data    =  general_reg.module_general_info[0].current_limit_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[0].current_limit_setup_reg.data);
    }
    break;
    case 0x67://M2电机限流
    {
        _reg_read.data    =  general_reg.module_general_info[1].current_limit_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[1].current_limit_setup_reg.data);
    }
    break;
    case 0x68://M3电机限流
    {
        _reg_read.data    =  general_reg.module_general_info[2].current_limit_setup_reg.data;
        _reg_read.size    = sizeof(general_reg.module_general_info[2].current_limit_setup_reg.data);
    }
    break;
    default: break;
    }
    return _reg_read;
}